#include "state_machine.h"
#include <stdio.h>
#include <string.h>

// 全局状态机实例
static GimbalControl_t gimbal_control = {0};

// 初始化状态机
void StateMachine_Init(StateMachine_t *sm)
{
    if (sm == NULL) return;
    
    memset(sm, 0, sizeof(StateMachine_t));
    
    // 初始化为等待任务选择状态
    sm->system_state = SYSTEM_WAIT_TASK_SELECT;
    sm->current_task = TASK_NONE;
    sm->selected_task = TASK_NONE;
    sm->is_running = true;
    sm->task_selection_received = false;
    
    // 初始化云台控制结构
    gimbal_control.x_position = 0;
    gimbal_control.y_position = 0;
    gimbal_control.laser_enabled = false;
    gimbal_control.x_motor_moving = false;
    gimbal_control.y_motor_moving = false;
    
    // 向串口屏发送任务列表
    printf("System: Waiting for task selection from screen...\n");
}

// 状态机主运行函数
void StateMachine_Run(StateMachine_t *sm)
{
    if (sm == NULL || !sm->is_running) return;
    
    switch (sm->system_state) {
        case SYSTEM_WAIT_TASK_SELECT:
            System_WaitTaskSelect_Handler(sm);
            break;
        case SYSTEM_TASK_RUNNING:
            System_TaskRunning_Handler(sm);
            break;
        case SYSTEM_TASK_COMPLETE:
            System_TaskComplete_Handler(sm);
            break;
        case SYSTEM_ERROR:
            printf("System Error State\n");
            sm->is_running = false;
            break;
        default:
            sm->system_state = SYSTEM_WAIT_TASK_SELECT;
            break;
    }
}

// 系统状态处理函数
void System_WaitTaskSelect_Handler(StateMachine_t *sm)
{
    // 检查是否接收到任务选择
    if (sm->task_selection_received && sm->selected_task != TASK_NONE) {
        printf("System: Task %d selected, starting execution...\n", sm->selected_task);
        
        // 切换到任务运行状态
        sm->current_task = sm->selected_task;
        sm->system_state = SYSTEM_TASK_RUNNING;
        sm->task_selection_received = false;
        
        // 根据任务类型初始化对应状态
        switch (sm->current_task) {
            case TASK_1:
                sm->task1_state = TASK1_GIMBAL_RESET;
                printf("Start Task 1: Aim at any position in the field\n");
                break;
            case TASK_2:
                sm->task2_state = TASK2_GIMBAL_RESET;
                printf("Start Task 2: Aim at the designated position on the trajectory\n");
                break;
            case TASK_3:
                sm->task3_state = TASK3_GIMBAL_RESET;
                printf("Start Task 3: Continuous Aiming While Driving\n");
                break;
            default:
                sm->system_state = SYSTEM_ERROR;
                printf("Error: Invalid task selection\n");
                return;
        }
    }
}

void System_TaskRunning_Handler(StateMachine_t *sm)
{
    // 根据当前任务调用相应的处理函数
    switch (sm->current_task) {
        case TASK_1:
            Task1_Handler(sm);
            break;
        case TASK_2:
            Task2_Handler(sm);
            break;
        case TASK_3:
            Task3_Handler(sm);
            break;
        default:
            sm->system_state = SYSTEM_ERROR;
            return;
    }
    
    // 检查任务是否完成
    bool task_complete = false;
    switch (sm->current_task) {
				case TASK_NONE:
						break;
        case TASK_1:
            task_complete = (sm->task1_state == TASK1_COMPLETE);
            break;
        case TASK_2:
            task_complete = (sm->task2_state == TASK2_COMPLETE);
            break;
        case TASK_3:
            task_complete = (sm->task3_state == TASK3_COMPLETE);
            break;
    }
    
    if (task_complete) {
        printf("System: Task %d completed!\n", sm->current_task);
        sm->system_state = SYSTEM_TASK_COMPLETE;
    }
}

void System_TaskComplete_Handler(StateMachine_t *sm)
{
    static bool completion_sent = false;
    
    if (!completion_sent) {
        printf("Task %d is Completed", sm->current_task);
        completion_sent = true;
    }
    
    // 自动返回任务选择状态
    printf("System: Auto reset to task selection state\n");
    StateMachine_Reset(sm);
    completion_sent = false;
}

void Task1_Handler(StateMachine_t *sm)
{
    switch (sm->task1_state) {
        case TASK1_GIMBAL_RESET:
            printf("Task1: Gimbal Reset\n");
            Gimbal_Reset();
            gimbal_control.x_motor_moving = true;
            gimbal_control.y_motor_moving = true;
            
            if (Gimbal_task1()) {
                gimbal_control.x_motor_moving = false;
                gimbal_control.y_motor_moving = false;
                sm->task1_state = TASK1_FIXED_AIMING;
            }
            break;
            
        case TASK1_FIXED_AIMING:
            printf("Task1: Fixed Aiming\n");
            // 固定瞄准逻辑，直接瞄准靶心
            if (Gimbal_IsAtPosition()) {
                sm->task1_state = TASK1_LASER_ON;
            }
            break;
            
        case TASK1_LASER_ON:
            printf("Task1: Laser On\n");
            Laser_Enable(true);
            gimbal_control.laser_enabled = true;
            
            // 激光开启后直接进入下一状态
            sm->task1_state = TASK1_LASER_OFF;
            break;
            
        case TASK1_LASER_OFF:
            printf("Task1: Laser Off\n");
            Laser_Enable(false);
            gimbal_control.laser_enabled = false;
            sm->task1_state = TASK1_GIMBAL_RESET_AGAIN;
            break;
            
        case TASK1_GIMBAL_RESET_AGAIN:
            printf("Task1: Gimbal Reset Again\n");
            Gimbal_Reset();
            gimbal_control.x_motor_moving = true;
            gimbal_control.y_motor_moving = true;
            
            if (Gimbal_IsAtPosition()) {
                gimbal_control.x_motor_moving = false;
                gimbal_control.y_motor_moving = false;
                sm->task1_state = TASK1_COMPLETE;
            }
            break;
            
        case TASK1_COMPLETE:
            printf("Task1: Complete\n");
            sm->is_running = false;
            break;
            
        default:
            sm->task1_state = TASK1_GIMBAL_RESET;
            break;
    }
}

// 任务2处理函数
void Task2_Handler(StateMachine_t *sm)
{
    switch (sm->task2_state) {
        case TASK2_GIMBAL_RESET:
            printf("Task2: Gimbal Reset\n");
            Gimbal_Reset();
            gimbal_control.x_motor_moving = true;
            gimbal_control.y_motor_moving = true;
            
            if (Gimbal_IsAtPosition()) {
                gimbal_control.x_motor_moving = false;
                gimbal_control.y_motor_moving = false;
                sm->task2_state = TASK2_X_MOTOR_360_CW;
            }
            break;
            
        case TASK2_X_MOTOR_360_CW:
            printf("Task2: X Motor 360 CW\n");
            Gimbal_RotateX(360); // 顺时针旋转360度
            gimbal_control.x_motor_moving = true;
            
            if (Gimbal_IsRotationComplete()) {
                gimbal_control.x_motor_moving = false;
								sm->camera_stop_signal = true;
                sm->task2_state = TASK2_PREPARE_AIMING;
            }
            break;
            
        case TASK2_PREPARE_AIMING:
            printf("Task2: Prepare Aiming\n");
            // 等待摄像头处理模块发送停止信号
            if (sm->camera_stop_signal) {
                sm->task2_state = TASK2_ADJUST_GIMBAL;
                sm->camera_stop_signal = false; // 清除信号
            }
            break;
            
        case TASK2_ADJUST_GIMBAL:
            printf("Task2: Adjust Gimbal (X:%d, Y:%d)\n", sm->camera_x_offset, sm->camera_y_offset);
            // 根据摄像头反馈的偏移量调整云台位置
            Gimbal_SetPosition(sm->camera_x_offset, sm->camera_y_offset);
            gimbal_control.x_motor_moving = true;
            gimbal_control.y_motor_moving = true;
            
            if (Gimbal_IsAtPosition()) {
                gimbal_control.x_motor_moving = false;
                gimbal_control.y_motor_moving = false;
                sm->task2_state = TASK2_LASER_ON;
            }
            break;
            
        case TASK2_LASER_ON:
            printf("Task2: Laser On\n");
            Laser_Enable(true);
            gimbal_control.laser_enabled = true;
            
            // 激光开启后直接进入下一状态
            sm->task2_state = TASK2_LASER_OFF;
            break;
            
        case TASK2_LASER_OFF:
            printf("Task2: Laser Off\n");
            Laser_Enable(false);
            gimbal_control.laser_enabled = false;
            sm->task2_state = TASK2_GIMBAL_RESET_AGAIN;
            break;
            
        case TASK2_GIMBAL_RESET_AGAIN:
            printf("Task2: Gimbal Reset Again\n");
            Gimbal_Reset();
            gimbal_control.x_motor_moving = true;
            gimbal_control.y_motor_moving = true;
            
            if (Gimbal_IsAtPosition()) {
                gimbal_control.x_motor_moving = false;
                gimbal_control.y_motor_moving = false;
                sm->task2_state = TASK2_COMPLETE;
            }
            break;
            
        case TASK2_COMPLETE:
            printf("Task2: Complete\n");
            sm->is_running = false;
            break;
            
        default:
            sm->task2_state = TASK2_GIMBAL_RESET;
            break;
    }
}

// 任务3处理函数
void Task3_Handler(StateMachine_t *sm)
{
    switch (sm->task3_state) {
        case TASK3_GIMBAL_RESET:
            printf("Task3: Gimbal Reset\n");
            Gimbal_Reset();
            gimbal_control.x_motor_moving = true;
            gimbal_control.y_motor_moving = true;
            
            if (Gimbal_IsAtPosition()) {
                gimbal_control.x_motor_moving = false;
                gimbal_control.y_motor_moving = false;
                sm->task3_state = TASK3_X_MOTOR_45_CW;
            }
            break;
            
        case TASK3_X_MOTOR_45_CW:
            printf("Task3: X Motor 45 CW\n");
            Gimbal_RotateX(45); // 顺时针旋转45度
            gimbal_control.x_motor_moving = true;
            
            if (Gimbal_IsRotationComplete()) {
                gimbal_control.x_motor_moving = false;
                sm->task3_state = TASK3_ADJUST_GIMBAL_1;
            }
            break;
            
        case TASK3_ADJUST_GIMBAL_1:
            printf("Task3: Adjust Gimbal 1 (X:%d, Y:%d)\n", sm->camera_x_offset, sm->camera_y_offset);
            Gimbal_SetPosition(sm->camera_x_offset, sm->camera_y_offset);
            gimbal_control.x_motor_moving = true;
            gimbal_control.y_motor_moving = true;
            
            if (Gimbal_IsAtPosition()) {
                gimbal_control.x_motor_moving = false;
                gimbal_control.y_motor_moving = false;
                sm->task3_state = TASK3_LASER_ON;
            }
            break;
            
        case TASK3_LASER_ON:
            printf("Task3: Laser On\n");
            Laser_Enable(true);
            gimbal_control.laser_enabled = true;
            
            // 等待小车旋转90度的信号
            if (sm->car_position_signal) {
                sm->task3_state = TASK3_X_MOTOR_90_CW_1;
                sm->car_position_signal = false;
            }
            break;
            
        case TASK3_X_MOTOR_90_CW_1:
            printf("Task3: X Motor 90 CW (1st)\n");
            Gimbal_RotateX(90);
            gimbal_control.x_motor_moving = true;
            
            if (Gimbal_IsRotationComplete()) {
                gimbal_control.x_motor_moving = false;
                sm->task3_state = TASK3_ADJUST_GIMBAL_2;
            }
            break;
            
        case TASK3_ADJUST_GIMBAL_2:
            printf("Task3: Adjust Gimbal 2 (X:%d, Y:%d)\n", sm->camera_x_offset, sm->camera_y_offset);
            Gimbal_SetPosition(sm->camera_x_offset, sm->camera_y_offset);
            gimbal_control.x_motor_moving = true;
            gimbal_control.y_motor_moving = true;
            
            if (Gimbal_IsAtPosition()) {
                gimbal_control.x_motor_moving = false;
                gimbal_control.y_motor_moving = false;
                // 等待下一个小车位置信号
                if (sm->car_position_signal) {
                    sm->task3_state = TASK3_X_MOTOR_90_CW_2;
                    sm->car_position_signal = false;
                }
            }
            break;
            
        case TASK3_X_MOTOR_90_CW_2:
            printf("Task3: X Motor 90 CW (2nd)\n");
            Gimbal_RotateX(90);
            gimbal_control.x_motor_moving = true;
            
            if (Gimbal_IsRotationComplete()) {
                gimbal_control.x_motor_moving = false;
                sm->task3_state = TASK3_ADJUST_GIMBAL_3;
            }
            break;
            
        case TASK3_ADJUST_GIMBAL_3:
            printf("Task3: Adjust Gimbal 3 (X:%d, Y:%d)\n", sm->camera_x_offset, sm->camera_y_offset);
            Gimbal_SetPosition(sm->camera_x_offset, sm->camera_y_offset);
            gimbal_control.x_motor_moving = true;
            gimbal_control.y_motor_moving = true;
            
            if (Gimbal_IsAtPosition()) {
                gimbal_control.x_motor_moving = false;
                gimbal_control.y_motor_moving = false;
                if (sm->car_position_signal) {
                    sm->task3_state = TASK3_X_MOTOR_90_CW_3;
                    sm->car_position_signal = false;
                }
            }
            break;
            
        case TASK3_X_MOTOR_90_CW_3:
            printf("Task3: X Motor 90 CW (3rd)\n");
            Gimbal_RotateX(90);
            gimbal_control.x_motor_moving = true;
            
            if (Gimbal_IsRotationComplete()) {
                gimbal_control.x_motor_moving = false;
                sm->task3_state = TASK3_ADJUST_GIMBAL_4;
            }
            break;
            
        case TASK3_ADJUST_GIMBAL_4:
            printf("Task3: Adjust Gimbal 4 (X:%d, Y:%d)\n", sm->camera_x_offset, sm->camera_y_offset);
            Gimbal_SetPosition(sm->camera_x_offset, sm->camera_y_offset);
            gimbal_control.x_motor_moving = true;
            gimbal_control.y_motor_moving = true;
            
            if (Gimbal_IsAtPosition()) {
                gimbal_control.x_motor_moving = false;
                gimbal_control.y_motor_moving = false;
                if (sm->car_position_signal) {
                    sm->task3_state = TASK3_X_MOTOR_90_CW_4;
                    sm->car_position_signal = false;
                }
            }
            break;
            
        case TASK3_X_MOTOR_90_CW_4:
            printf("Task3: X Motor 90 CW (4th)\n");
            Gimbal_RotateX(90);
            gimbal_control.x_motor_moving = true;
            
            if (Gimbal_IsRotationComplete()) {
                gimbal_control.x_motor_moving = false;
                sm->task3_state = TASK3_ADJUST_GIMBAL_5;
            }
            break;
            
        case TASK3_ADJUST_GIMBAL_5:
            printf("Task3: Adjust Gimbal 5 (X:%d, Y:%d)\n", sm->camera_x_offset, sm->camera_y_offset);
            Gimbal_SetPosition(sm->camera_x_offset, sm->camera_y_offset);
            gimbal_control.x_motor_moving = true;
            gimbal_control.y_motor_moving = true;
            
            if (Gimbal_IsAtPosition()) {
                gimbal_control.x_motor_moving = false;
                gimbal_control.y_motor_moving = false;
                // 检查是否到达终点
                if (sm->car_rotation_count >= 1) { // 小车完成1圈
                    sm->task3_state = TASK3_LASER_OFF;
                }
            }
            break;
            
        case TASK3_LASER_OFF:
            printf("Task3: Laser Off (Reached destination)\n");
            Laser_Enable(false);
            gimbal_control.laser_enabled = false;
            sm->task3_state = TASK3_GIMBAL_RESET_AGAIN;
            break;
            
        case TASK3_GIMBAL_RESET_AGAIN:
            printf("Task3: Gimbal Reset Again\n");
            Gimbal_Reset();
            gimbal_control.x_motor_moving = true;
            gimbal_control.y_motor_moving = true;
            
            if (Gimbal_IsAtPosition()) {
                gimbal_control.x_motor_moving = false;
                gimbal_control.y_motor_moving = false;
                sm->task3_state = TASK3_COMPLETE;
            }
            break;
            
        case TASK3_COMPLETE:
            printf("Task3: Complete\n");
            sm->is_running = false;
            break;
            
        default:
            sm->task3_state = TASK3_GIMBAL_RESET;
            break;
    }
}

// 设置任务选择
void StateMachine_SetTaskSelection(StateMachine_t *sm, TaskType_t task_type)
{
    if (sm == NULL) return;
    
    if (task_type >= TASK_1 && task_type <= TASK_3) {
        sm->selected_task = task_type;
        sm->task_selection_received = true;
        printf("System: Received task selection: %d\n", task_type);
    } else {
        printf("System: Invalid task selection: %d\n", task_type);
    }
}

// 复位状态机
void StateMachine_Reset(StateMachine_t *sm)
{
    if (sm == NULL) return;
    
    // 关闭激光和复位云台
    Laser_Enable(false);
    Gimbal_Reset();
    
    // 重新初始化状态机
    StateMachine_Init(sm);
}

// 设置摄像头停止信号
void StateMachine_SetCameraStopSignal(StateMachine_t *sm, bool signal)
{
    if (sm == NULL) return;
    
    sm->camera_stop_signal = signal;
}

// 设置摄像头偏移值
void StateMachine_SetCameraOffset(StateMachine_t *sm, int16_t x_offset, int16_t y_offset)
{
    if (sm == NULL) return;
    
    sm->camera_x_offset = x_offset;
    sm->camera_y_offset = y_offset;
}

//// 设置小车位置信号
//void StateMachine_SetCarPositionSignal(StateMachine_t *sm, bool signal)
//{
//    if (sm == NULL) return;
//    
//    sm->car_position_signal = signal;
//}

// 检查任务是否完成
bool StateMachine_IsTaskComplete(StateMachine_t *sm)
{
    if (sm == NULL) return true;
    
    return (sm->system_state == SYSTEM_TASK_COMPLETE || !sm->is_running);
}

//// 串口中断处理函数
//void UART_IRQHandler(StateMachine_t *sm)
//{
//    if (sm == NULL) return;
//    
//    static uint8_t rx_buffer[32];
//    static uint8_t rx_index = 0;
//    
//    if (UART_HasData()) {
//        uint8_t received_byte = UART_ReadByte();
//        
//        // 简单的协议解析: 接收到数字1、2、3表示任务选择
//        if (received_byte >= '1' && received_byte <= '3') {
//            TaskType_t selected_task = (TaskType_t)(received_byte - '0');
//            StateMachine_SetTaskSelection(sm, selected_task);
//            rx_index = 0; // 重置缓冲区
//        }
//        // 接收到'R'或'r'表示复位
//        else if (received_byte == 'R' || received_byte == 'r') {
//            printf("System: Reset command received\n");
//            Screen_SendMessage("系统复位中...");
//            StateMachine_Reset(sm);
//            rx_index = 0;
//        }
//        // 接收到其他字符，存储到缓冲区进行进一步解析
//        else if (received_byte != '\r' && received_byte != '\n') {
//            if (rx_index < sizeof(rx_buffer) - 1) {
//                rx_buffer[rx_index++] = received_byte;
//            }
//        }
//        // 接收到换行符，处理完整命令
//        else if (received_byte == '\n' && rx_index > 0) {
//            rx_buffer[rx_index] = '\0';
//            
//            // 解析命令字符串
//            if (strcmp((char*)rx_buffer, "STATUS") == 0) {
//                // 发送状态信息
//                char status_msg[128];
//                snprintf(status_msg, sizeof(status_msg), 
//                        "系统状态:%d 当前任务:%d 状态:%s", 
//                        sm->system_state, sm->current_task, 
//                        StateMachine_GetCurrentStateString(sm));
//                Screen_SendMessage(status_msg);
//            }
//            else if (strcmp((char*)rx_buffer, "HELP") == 0) {
//                Screen_SendMessage("命令: 1/2/3-选择任务 R-复位 STATUS-状态 HELP-帮助");
//            }
//            
//            rx_index = 0; // 重置缓冲区
//        }
//    }
//}

//// 获取当前状态字符串
//const char* StateMachine_GetCurrentStateString(StateMachine_t *sm)
//{
//    if (sm == NULL) return "NULL";
//    
//    static char state_str[64];
//    
//    switch (sm->system_state) {
//        case SYSTEM_WAIT_TASK_SELECT:
//            return "System: Waiting for task selection";
//        case SYSTEM_TASK_RUNNING:
//            switch (sm->current_task) {
//                case TASK_1:
//                    switch (sm->task1_state) {
//                        case TASK1_GIMBAL_RESET: return "Task1: Gimbal Reset";
//                        case TASK1_FIXED_AIMING: return "Task1: Fixed Aiming";
//                        case TASK1_LASER_ON: return "Task1: Laser On";
//                        case TASK1_LASER_OFF: return "Task1: Laser Off";
//                        case TASK1_GIMBAL_RESET_AGAIN: return "Task1: Gimbal Reset Again";
//                        case TASK1_COMPLETE: return "Task1: Complete";
//                        default: return "Task1: Unknown";
//                    }
//                    break;
//                    
//                case TASK_2:
//                    switch (sm->task2_state) {
//                        case TASK2_GIMBAL_RESET: return "Task2: Gimbal Reset";
//                        case TASK2_X_MOTOR_360_CW: return "Task2: X Motor 360 CW";
//                        case TASK2_PREPARE_AIMING: return "Task2: Prepare Aiming";
//                        case TASK2_ADJUST_GIMBAL: return "Task2: Adjust Gimbal";
//                        case TASK2_LASER_ON: return "Task2: Laser On";
//                        case TASK2_LASER_OFF: return "Task2: Laser Off";
//                        case TASK2_GIMBAL_RESET_AGAIN: return "Task2: Gimbal Reset Again";
//                        case TASK2_COMPLETE: return "Task2: Complete";
//                        default: return "Task2: Unknown";
//                    }
//                    break;
//                    
//                case TASK_3:
//                    snprintf(state_str, sizeof(state_str), "Task3: State %d", sm->task3_state);
//                    return state_str;
//                    
//                default:
//                    return "Unknown Task";
//            }
//            break;
//        case SYSTEM_TASK_COMPLETE:
//            return "System: Task Complete";
//        case SYSTEM_ERROR:
//            return "System: Error";
//        default:
//            return "System: Unknown State";
//    }
//}

//// 打印状态机状态
//void StateMachine_PrintStatus(StateMachine_t *sm)
//{
//    if (sm == NULL) return;
//    
//    printf("=== State Machine Status ===\n");
//    printf("System State: %d\n", sm->system_state);
//    printf("Current Task: %d\n", sm->current_task);
//    printf("Selected Task: %d\n", sm->selected_task);
//    printf("Current State: %s\n", StateMachine_GetCurrentStateString(sm));
//    printf("Is Running: %s\n", sm->is_running ? "Yes" : "No");
//    printf("Task Selection Received: %s\n", sm->task_selection_received ? "Yes" : "No");
//    printf("Camera Stop Signal: %s\n", sm->camera_stop_signal ? "Yes" : "No");
//    printf("Car Position Signal: %s\n", sm->car_position_signal ? "Yes" : "No");
//    printf("Camera Offset: X=%d, Y=%d\n", sm->camera_x_offset, sm->camera_y_offset);
//    printf("Gimbal Position: X=%d, Y=%d\n", gimbal_control.x_position, gimbal_control.y_position);
//    printf("Laser Enabled: %s\n", gimbal_control.laser_enabled ? "Yes" : "No");
//    printf("Motors Moving: X=%s, Y=%s\n", 
//           gimbal_control.x_motor_moving ? "Yes" : "No",
//           gimbal_control.y_motor_moving ? "Yes" : "No");
//    printf("============================\n");
//}
